
;	File:  intrvec.s
;
;	Programer: Visual.Wei 
;	Date: 2006-3-20

        INCLUDE kxarm.h

;----------------------------------------------
; We use this magic number to check whether stub installation is successful
MAGIC_SIGN				EQU	0x55AA55AA
;----------------------------------------------
        TEXTAREA

;----------------vectors------offset: 0x00----------------
_stub_start	B	INT_Swi			;  short jump
;----------magic bumber---offset: 0x4-----------------
magic           DCD     MAGIC_SIGN
;-----long jump function address-------
swi_h		DCD	0		
;-----------------swi ISR----------------
; this handler must be physical address independent
;
INT_Swi
		stmfd sp!,{r0-r12,lr}	;backup 
		
		add r4, pc, #(swi_h - . - 8)
		add lr, pc, #4
		ldr r5,[r4]
		mov pc,r5

		ldmfd sp!,{r0-r12,pc}	;restore

_stub_end

;----------------_get_trap_size()----------------------------------
		LEAF_ENTRY _get_trap_size

		mov	r0, #(_stub_end-_stub_start)
		bx	lr
		ENTRY_END
;------------------------------------------------------------

;----------------_trap_init(insert address, jump address)----------------------------------
; backup old text code, then relace it with shell code or stub code.
; And check the code moving is OK. 
		LEAF_ENTRY _trap_init
		;---- backup --
		; r0 will be used several times. So store it into stack is a must.
		; r1 contains long jump address
		stmfd sp!,{r0-r1}
		;---- install jump address --
		ldr	r3,=swi_h
		str	r1,[r3]
		;---- backup old code --
		ldr	r2, =old_code		
		ldr	r3, =old_code_end
4		ldr	r1, [r0], #4
		str	r1, [r2], #4
		cmp	r2, r3
		blt	%B4
		;---- move code --
		ldr	r0, [sp]	;get 'insert address'
		ldr	r2, =_stub_start		
		ldr	r3, =_stub_end
1		ldr	r1, [r2], #4
		str	r1, [r0], #4
		cmp	r2, r3
		blt	%B1
		;---- check magic signature --
		ldr	r0, [sp]	;get 'insert address'
		add	r2, r0, #(magic-_stub_start)
		ldr	r3, [r2]
		ldr	r2, magic
		cmp	r2,r3
		beq	%f2
		mov	r0,#0
		;---- restore --
2		add	sp, sp, #8
		bx	lr
		ENTRY_END
;------------------------------------------------------------

;----------------_trap_deinit(insert address)----------------------------------
		LEAF_ENTRY _trap_deinit

		;---- restore old code --
		ldr	r2, =old_code		
		ldr	r3, =old_code_end
4		ldr	r1, [r2], #4
		str	r1, [r0], #4
		cmp	r2, r3
		blt	%B4

		bx	lr
		ENTRY_END
;------------------------------------------------------------
;---------------------------------------------------
 AREA |.data|, DATA
old_code	SPACE 	(_stub_end-_stub_start)
old_code_end
;----------magic bumber--------------------
spliter           DCD     0xDDDDDDDD
        END
